#include <pololu/3pi.h>
#include <math.h>
#include <stdlib.h>

// make ; avrdude -p m328p -c usbasp -P usb -U flash:w:test.hex

void rampUp(int from, int to, int step, int delay){
  int i;
  for(i=from; step>0 ? i<to : i>to; i+=step){
    set_motors(i,i);
    delay_ms(delay);
  }
  set_motors(to,to);
}

void turn90(){
  int i;
  for(i=0; i<100; i+=10){
    set_motors(i,-i);
    delay_ms(14);
  }
  delay_ms(40);
  for(i=100; i>0; i-=10){
    set_motors(i,-i);
    delay_ms(14);
  }

//		set_motors(255,-255);
//		delay_ms(70); - at maximum speed wheels sometimes slip
		set_motors(0,0);
}


void calibrateSensorsGauss(unsigned int senLo[5], unsigned int senHi[5]){
  int i;
  int j;

  int n = 0;
  float mean[5];
  float M2[5];
  for( j=0; j<5; j++ ){
    mean[j]=0;
    M2[j]=0;
  }
  for( i=0; i<100; i++ ){
//    if( i==0 ) set_motors(-30,30);
//    if( i==25 ) set_motors(30,-30);
//    if( i==75 ) set_motors(-30,30);

    unsigned int sen[5];
    read_line_sensors(sen,IR_EMITTERS_ON);

    n = n + 1;
    for( j=0; j<5; j++ ){
        float x = sen[j];
        float delta = x - mean[j];
        mean[j] = mean[j] + delta/n;
        M2[j] = M2[j] + delta*(x - mean[j]);
    }
    delay_ms(4);
  }
//  set_motors(0,0);

  for( j=0; j<5; j++ ){
    float d = 6*sqrt(M2[j]/(n - 1));
    senLo[j] = (int)(mean[j] - d);
    senHi[j] = (int)(mean[j] + d);
  }


}

void calibrateSensors(char c, unsigned int sen[5]){
  do{
        read_line_sensors(sen,IR_EMITTERS_ON);
	clear();                  // clear the LCD
	lcd_goto_xy(0, 0);
        int i;
        print_character(c);
        for( i=0; i<2; i++ ){
          print_character(' ');
	  print_long(sen[i]/16);
        }
        lcd_goto_xy(0, 1);
        print_long(sen[2]/16);
        print_character(' ');
        print_long(sen[3]/16);
        print_character(' ');
        print_long(sen[4]/16);
	delay_ms(100);
  }while( ! button_is_pressed(BUTTON_B) );
  while(button_is_pressed(BUTTON_B));
  play_frequency(6000, 150, 15);
}

void printSensorDiff(unsigned int senIn[5], unsigned int senOut[5]){
        clear(); 
	lcd_goto_xy(0, 0);
        int i;
        print_character('D');
        for( i=0; i<2; i++ ){
          print_character(' ');
	  print_long(((int)senOut[i]-(int)senIn[i])/16);
        }
        lcd_goto_xy(0, 1);
        print_long(((int)senOut[2]-(int)senIn[2])/16);
        print_character(' ');
        print_long(((int)senOut[3]-(int)senIn[3])/16);
        print_character(' ');
        print_long(((int)senOut[4]-(int)senIn[4])/16);
}

void checkOutsideVisualize(unsigned int senIn[5], unsigned int senOut[5], unsigned int sen[5]){
        clear(); 
	lcd_goto_xy(0, 0);
	

	//unsigned int sen[5];
        //read_line_sensors(sen,IR_EMITTERS_ON);
	int i;
	for( i=0; i<5; i++ ){
		// senIn is less than senOut
		int max = ((int)senOut[i]+(int)senIn[i])/2;
		if( sen[i] >= max ){
			        print_character('*');
		}else{
			        print_character('_');
		}
 	}
}

int checkOutside(unsigned int senIn[5], unsigned int senOut[5]){
	unsigned int sen[5];
        read_line_sensors(sen,IR_EMITTERS_ON);
	int i;
	for( i=0; i<5; i++ ){
		// senIn is less than senOut
		int max = ((int)senOut[i]+(int)senIn[i])/2;
		if( sen[i] >= max ){
                        checkOutsideVisualize(senIn, senOut, sen);
			return 1;
		}
 	}
	return 0;
}

void checkOutsideVisualizeG(unsigned int senLo[5], unsigned int senHi[5], unsigned int sen[5]){ // Gauss version
        clear(); 
	lcd_goto_xy(0, 0);
	
	int i;
	for( i=0; i<5; i++ ){
		if( sen[i] < senLo[i] || sen[i] > senHi[i] ){
			        print_character('*');
		}else{
			        print_character('_');
		}
 	}
}

void visualize01(unsigned int sen[5]){ 
        clear(); 
	lcd_goto_xy(0, 0);
	
	int i;
	for( i=0; i<5; i++ ){
		if( sen[i] == 1 ){
			        print_character('*');
		}else{
			        print_character('_');
		}
 	}
}

int checkOutsideG(unsigned int senLo[5], unsigned int senHi[5]){ // Gauss version
	unsigned int sen[5];
        read_line_sensors(sen,IR_EMITTERS_ON);
	int i;
	for( i=0; i<5; i++ ){
		if( sen[i] < senLo[i] || sen[i] > senHi[i] ){
                        checkOutsideVisualizeG(senLo, senHi, sen);
			return 1;
		}
 	}
	return 0;
}

void checkOutsideG5(unsigned int senLo[5], unsigned int senHi[5], unsigned int res[5]){ // Gauss version
	unsigned int sen[5];
        read_line_sensors(sen,IR_EMITTERS_ON);
	int i;
	for( i=0; i<5; i++ ){
		res[i]=0;
		if( sen[i] < senLo[i] || sen[i] > senHi[i] ){
			res[i] = 1;
		}
 	}
}

int clamp(int x, int a, int b){
  if( x<a ) return a;
  if( x>b ) return b;
  return x;
}


void sensors(){
  pololu_3pi_init(1500);
/*
  unsigned int senIn[5]; // an array to hold sensor values
  unsigned int senOut[5]; // an array to hold sensor values
  
  calibrateSensors('I',senIn);
  calibrateSensors('O',senOut);
  printSensorDiff(senIn, senOut);

  while(!button_is_pressed(BUTTON_B));
  while(button_is_pressed(BUTTON_B));
  play_frequency(6000, 150, 15);
  delay_ms(1000);
*/
 unsigned int senLo[5];
 unsigned int senHi[5];

int max=20;
int lspeed=max, rspeed=max;
 set_motors(max,max);
 calibrateSensorsGauss(senLo, senHi);
 play_frequency(6000, 150, 15);

/*
  int outside=0;
  while(outside<10){
    //if( checkOutside(senIn, senOut)==0 ){
    if( checkOutsideG(senLo, senHi)==0 ){
      outside=0;
    }else{
      outside++;
    }
  }
*/
  int step=0;
  while(1==1){
    unsigned int sens[5];
    checkOutsideG5(senLo, senHi, sens);

	    if( sens[0]==1 && sens[1]==1 ){
	      rspeed -= 2; 
	    }else{
	      rspeed += 1;
	    }

	    if( sens[3]==1 && sens[4]==1 ){
	      rspeed -= 2;
	    }else{
	      rspeed += 1;
	    }
	    rspeed = clamp(rspeed, -max, max);
	    lspeed = clamp(lspeed, -max, max);

    set_motors(lspeed,rspeed);

    step++;
    if( step==100 ){
      step=0;
      visualize01(sens);
      //play_frequency(6000, 10, 15);
    }
  }
  
  set_motors(0,0);
  play_frequency(6000, 150, 15);

  while(!button_is_pressed(BUTTON_B));
  while(button_is_pressed(BUTTON_B));
  for(;;){
    unsigned int sen[5];
    read_line_sensors(sen,IR_EMITTERS_ON);
    //    checkOutsideVisualize(senIn, senOut, sen);
    checkOutsideVisualizeG(senLo, senHi, sen);
    delay_ms(300);
  }

  for(;;);
}

int main()
{
	clear();                  // clear the LCD
	lcd_goto_xy(0, 0);
	print("BatteRY");

	int bat = read_battery_millivolts();

	lcd_goto_xy(0, 1);
	print_long(bat);
	print("mV");


	// wait for all buttons to be released, then a press
	// ANY_BUTTON
	while(button_is_pressed(BUTTON_B));
	/*char button =*/ wait_for_button_press(BUTTON_B);
	long seed=0;
	while(button_is_pressed(BUTTON_B)) seed++;
	srandom(seed);
	delay_ms(1000);

	sensors();
	return 0;

	//if(button & BUTTON_A)

}


// Local Variables: **
// mode: C **
// c-basic-offset: 4 **
// tab-width: 4 **
// indent-tabs-mode: t **
// end: **
